#include "StdAfx.h"
#include "NaiveRouter.h"

using namespace std;

NaiveRouter::NaiveRouter(void)
{
}

NaiveRouter::~NaiveRouter(void)
{
}

std::vector<iPos> NaiveRouter::route(std::vector< std::vector< char >> map, fPos nowPos, iPos targetPos)
{
	int i;

	vector<iPos> path;

	float k = fabs((targetPos.y - nowPos.y)/(targetPos.x - nowPos.x));

	int nstep = 0;
	float dx, dy;
	if (k>1)
	{
		nstep = abs(floor(nowPos.y) - targetPos.y);
	} 
	else
	{
		nstep = abs(floor(nowPos.x) - targetPos.x);
	}
	dx = float(targetPos.x - floor(nowPos.x))/nstep;
	dy = float(targetPos.y - floor(nowPos.y))/nstep;

	iPos node;
	fPos fnode;
	fnode.x = nowPos.x;
	fnode.y = nowPos.y;
	node.x = int(fnode.x);
	node.y = int(fnode.y);
	path.push_back(node);

	for (i=0; i<nstep; i++)
	{
		fnode.x = (fnode.x+dx);
		fnode.y = (fnode.y+dy);

		node.x = ROUND(fnode.x);
		node.y = ROUND(fnode.y);

		path.push_back(node);
	}

	return path;
}